#include "test_helix.h"
#include "Geom_HelixCurve.h"

#include <HelixGeom_BuilderHelix.hxx>
#include <HelixGeom_HelixCurve.hxx>
#include <HelixBRep_BuilderHelix.hxx>
#include <HelixGeom_Tools.hxx>
#include <GeomAPI_PointsToBSpline.hxx>
#include <GeomAPI_PointsToBSplineSurface.hxx>
#include <BRep_CurveOnSurface.hxx>
#include <Geom_BSplineCurve.hxx>
#include <Geom_BSplineSurface.hxx>
#include <Geom_ConicalSurface.hxx>
#include <Geom_CylindricalSurface.hxx>
#include <Geom2d_Line.hxx>
#include <Geom2d_TrimmedCurve.hxx>
#include <Geom_TrimmedCurve.hxx>
#include <BRepBuilderAPI_MakeVertex.hxx>
#include <BRepBuilderAPI_MakeEdge.hxx>
#include <BRepBuilderAPI_MakeFace.hxx>
#include <BRepBuilderAPI_MakePolygon.hxx>
#include <TopoDS_Edge.hxx>
#include <TopoDS_Face.hxx>
#include <TopoDS_Wire.hxx>
#include <Adaptor3d_CurveOnSurface.hxx>
#include <GeomAdaptor_Curve.hxx>
#include <Geom2dAdaptor_Curve.hxx>
#include <GeomAdaptor_Surface.hxx>
#include <Extrema_ExtCC.hxx>
#include <Extrema_ExtCS.hxx>
#include <GCPnts_AbscissaPoint.hxx>

#include <iostream>

// 用于求交的基准线
Handle(Geom_Curve) MakeBaseCurve()
{
    std::vector<gp_Pnt> pts{
        gp_Pnt(0, 0, -10),
        gp_Pnt(2, 3, -8),
        gp_Pnt(5, 5, -5),
        gp_Pnt(0, 10, 5),
        gp_Pnt(-5, 0, 10),
        gp_Pnt(0, -5, 20),
        gp_Pnt(10, 1, -1),
    };

    TColgp_Array1OfPnt occ_pnts(1, int(pts.size()));
    for (size_t i = 0; i < pts.size(); i++)
    {
        occ_pnts.SetValue(int(i + 1), pts[i]);
    }

    GeomAPI_PointsToBSpline tobs(occ_pnts, 3, GeomAbs_C2, 1.0e-6);
    if (tobs.IsDone())
    {
        return tobs.Curve();
    }
    else
    {
        return nullptr;
    }
}

// 用于求交的基准线
Handle(Geom_Surface) MakeBaseSurf()
{
    std::vector<std::vector<gp_Pnt>> pts{
        {
            gp_Pnt(-6, 0, -2),
            gp_Pnt(16, 0, 4),
            gp_Pnt(33, 0, 8),
            gp_Pnt(40, 1, 3),
            gp_Pnt(47, 0, -1),
        },
        {
            gp_Pnt(-10, 22, 12),
            gp_Pnt(15, 26, 9),
            gp_Pnt(39, 30, 5),
            gp_Pnt(46, 33, 8),
            gp_Pnt(50, 37, 11),
        },
        {
            gp_Pnt(6, 50, 46),
            gp_Pnt(15, 40, 48),
            gp_Pnt(24, 30, 50),
            gp_Pnt(30, 30, 50),
            gp_Pnt(36, 31, 51),
        }
    };

    TColgp_Array2OfPnt occ_pnts(1, int(pts.size()), 1, int(pts[0].size()));
    for (size_t i = 0; i < pts.size(); i++)
    {
        for (size_t j = 0; j < pts[i].size(); j++)
        {
            occ_pnts.SetValue(int(i + 1), int(j + 1), pts[i][j]);
        }
    }
    GeomAPI_PointsToBSplineSurface tobs(occ_pnts, 3, 3, GeomAbs_C2, 1.0e-6);
    if (tobs.IsDone())
    {
        return tobs.Surface();
    }
    else
    {
        return nullptr;
    }

}

Handle(Adaptor3d_Curve) MakeHelixAdaptorCurveOCC(const HelixData& d)
{
    Handle(HelixGeom_HelixCurve) curve = new HelixGeom_HelixCurve();
    const double taper = 2 * std::atan(2 * M_PI * d.taperFactor / d.pitch);
    curve->Load(d.tStart, d.tEnd, d.pitch, d.rStart, taper * 0.5, d.ccw);
    return curve;
}

// OCC 方法
Handle(Geom_BSplineCurve) MakeHelixCurveOCC(const HelixData& d)
{
    HelixGeom_BuilderHelix helix_builder;
    helix_builder.SetCurveParameters(d.tStart, d.tEnd, d.pitch, d.rStart, d.taperFactor, d.ccw);
    helix_builder.SetApproxParameters(GeomAbs_C2, 3, 1);
    helix_builder.SetTolerance(Precision::Confusion());
    helix_builder.SetPosition(d.loc);
    helix_builder.Perform();
    std::cout << "Tolerance reached " << helix_builder.ToleranceReached() << std::endl;

    const TColGeom_SequenceOfCurve& curves = helix_builder.Curves();
    Handle(Geom_BSplineCurve) helix_curve;
    for (auto it = curves.begin(); it != curves.end(); ++it)
    {
        helix_curve = Handle(Geom_BSplineCurve)::DownCast(*it);
    }
    return helix_curve;
}

// 方法1: 自行实现的螺旋线
Handle(Geom_Curve) MakeHelixCurve1(const HelixData& d)
{
    Handle(Geom_HelixCurve) helix = new Geom_HelixCurve(d.loc, d.rStart, d.pitch);
    Handle(Geom_TrimmedCurve) thelix = new Geom_TrimmedCurve(helix, d.tStart, d.tEnd);
    return thelix;
}

// 方法2: OCC 的 BRep_CurveOnSurface
Handle(BRep_CurveOnSurface) MakeHelixCurve2(const HelixData& d)
{
    const double taper = 2 * std::atan(2 * M_PI * d.taperFactor / d.pitch);
    Handle(Geom_Surface) surf;
    gp_Ax3 lcs = d.loc;
    if (!d.ccw)
    {
        lcs.YReverse();
    }
    if (std::fabs(taper) < gp::Resolution())
    {
        surf = new Geom_CylindricalSurface(lcs, d.rStart);
    }
    else
    {
        surf = new Geom_ConicalSurface(lcs, taper * 0.5, d.rStart); // V 参数沿表面
    }

    double surf_vstart = d.tStart;
    double surf_vend = surf_vstart + d.pitch / std::cos(taper) * (d.tEnd - d.tStart) / (2 * M_PI);
    double surf_ustart = d.tStart;
    double surf_uend = d.tEnd; // 旋转方向
    gp_Pnt2d line_start(surf_ustart, surf_vstart);
    gp_Pnt2d line_end(surf_uend, surf_vend);
    
    Handle(Geom2d_Line) line = new Geom2d_Line(line_start, gp_Vec2d(line_start, line_end));
    Handle(Geom2d_TrimmedCurve) trimmed_line =
        new Geom2d_TrimmedCurve(line, 0, line_start.Distance(line_end), false, false);
    if (!d.ccw)
    {
    }
    Handle(BRep_CurveOnSurface) helix_curve = new BRep_CurveOnSurface(trimmed_line, surf, TopLoc_Location());
    return helix_curve;
}

void MakePolygon(const Handle(Adaptor3d_Curve)& curve, const std::string& name,
                 std::vector<TopoDS_Shape>& shapes, std::vector<std::string>& names)
{
    if (curve)
    {
        const double t0 = curve->FirstParameter();
        const double t1 = curve->LastParameter();

        //double len = GCPnts_AbscissaPoint::Length(*curve);

        const int N = 500;

        BRepBuilderAPI_MakePolygon polygon;
        for (int i = 0; i < N; i++)
        {
            const double t = t0 + (t1 - t0) * double(i) / double(N);
            gp_Pnt p;
            curve->D0(t, p);
            polygon.Add(p);
        }
        if (polygon.IsDone())
        {
            shapes.emplace_back(polygon.Shape());
            names.emplace_back(name);
        }
    }
}

// 输出模型
void MakeEdge(const Handle(Geom_Curve)& c, const std::string& name,
              std::vector<TopoDS_Shape>& shapes, std::vector<std::string>& names)
{
    if (c)
    {
        BRepBuilderAPI_MakeEdge builder(c);
        if (builder.IsDone())
        {
            shapes.emplace_back(builder.Edge());
            names.emplace_back(name);
        }
    }
}

void MakeEdge(const Handle(BRep_CurveOnSurface)& c, const std::string& name,
              std::vector<TopoDS_Shape>& shapes, std::vector<std::string>& names)
{
    if (c)
    {
        BRepBuilderAPI_MakeEdge builder(c->PCurve(), c->Surface());
        if (builder.IsDone())
        {
            shapes.emplace_back(builder.Edge());
            names.emplace_back(name);
        }
    }
}

void MakeFace(const Handle(Geom_Surface)& s, const std::string& name,
              std::vector<TopoDS_Shape>& shapes, std::vector<std::string>& names)
{
    if (s)
    {
        BRepBuilderAPI_MakeFace builder(s, Precision::Confusion());
        if (builder.IsDone())
        {
            shapes.emplace_back(builder.Face());
            names.emplace_back(name);
        }
    }
}

void MakeLines(const std::vector<gp_Pnt>& ps1, const std::vector<gp_Pnt>& ps2, const std::string& name,
               std::vector<TopoDS_Shape>& shapes, std::vector<std::string>& names)
{
    for (size_t i = 0; i < ps1.size() && i < ps2.size(); i++)
    {
        BRepBuilderAPI_MakeEdge edge(ps1[i], ps2[i]);
        if (edge.IsDone())
        {
            shapes.emplace_back(edge.Edge());
            names.emplace_back(name + "_" + std::to_string(i) + "_dist" + std::to_string(ps1[i].Distance(ps2[i])));
        }
    }
}

void MakeVertices(const std::vector<gp_Pnt>& ps1, const std::string& name,
                  std::vector<TopoDS_Shape>& shapes, std::vector<std::string>& names)
{
    for (size_t i = 0; i < ps1.size(); i++)
    {
        BRepBuilderAPI_MakeVertex vtx(ps1[i]);
        if (vtx.IsDone())
        {
            shapes.emplace_back(vtx.Vertex());
            names.emplace_back(name + "_" + std::to_string(i) + "_交点");
        }
    }
}

// 求解计算
// 返回耗时，单位毫秒
int TestCurveCurveDistAdaptor(const Adaptor3d_Curve& c1, const Adaptor3d_Curve& c2,
                              std::vector<gp_Pnt>& ps1, std::vector<gp_Pnt>& ps2)
{
    ps1.clear();
    ps2.clear();

    std::clock_t start = std::clock();
    Extrema_ExtCC ext_cc(c1, c2);
    ext_cc.Perform();
    if (ext_cc.IsDone())
    {
        Extrema_POnCurv p1, p2;
        for (int i = 1; i <= ext_cc.NbExt(); i++)
        {
            ext_cc.Points(i, p1, p2);

            //ps1.emplace_back(p1.Value());
            //ps2.emplace_back(p2.Value());
        }
    }
    return std::clock() - start;
}

int TestCurveCurveDistMethod1(const Handle(Geom_Curve)& c1, const Handle(Geom_Curve)& c2,
                              std::vector<gp_Pnt>& ps1, std::vector<gp_Pnt>& ps2)
{
    GeomAdaptor_Curve ac1(c1);
    GeomAdaptor_Curve ac2(c2);
    return TestCurveCurveDistAdaptor(ac1, ac2, ps1, ps2);


    // OCC 曲线求距离
    //std::clock_t start = std::clock();
    //GeomAPI_ExtremaCurveCurve extrema(c1, c2);
    //if (extrema.NbExtrema() > 0)
    //{
    //    gp_Pnt p1, p2;
    //    for (int i = 1; i <= extrema.NbExtrema(); i++)
    //    {
    //        extrema.Points(i, p1, p2);
    //    }
    //}
    //return std::clock() - start;
}

int TestCurveCurveDistOCC(const Handle(Geom_BSplineCurve)& c1, const Handle(Geom_Curve)& c2,
                          std::vector<gp_Pnt>& ps1, std::vector<gp_Pnt>& ps2)
{
    GeomAdaptor_Curve ac1(c1);
    GeomAdaptor_Curve ac2(c2);
    return TestCurveCurveDistAdaptor(ac1, ac2, ps1, ps2);
}

int TestCurveCurveDistMethod2(const Handle(BRep_CurveOnSurface)& c1, const Handle(Geom_Curve)& c2,
                              std::vector<gp_Pnt>& ps1, std::vector<gp_Pnt>& ps2)
{
    Handle(Geom2dAdaptor_Curve) apc = new Geom2dAdaptor_Curve(c1->PCurve());
    Handle(GeomAdaptor_Surface) as = new GeomAdaptor_Surface(c1->Surface());

    Adaptor3d_CurveOnSurface ac1(apc, as);
    GeomAdaptor_Curve ac2(c2);
    return TestCurveCurveDistAdaptor(ac1, ac2, ps1, ps2);
}

// 线-面求交
// 返回耗时，单位毫秒
int TestCurveSurfIntersectAdaptor(const Adaptor3d_Curve& c, const Adaptor3d_Surface& s,
                                  std::vector<gp_Pnt>& ps1, std::vector<gp_Pnt>& ps2)
{
    ps1.clear();
    ps2.clear();

    std::clock_t start = std::clock();
    Extrema_ExtCS ext_cs(c, s, Precision::Confusion(), Precision::Confusion());
    if (ext_cs.IsDone())
    {
        Extrema_POnCurv p1;
        Extrema_POnSurf p2;
        for (int i = 1; i <= ext_cs.NbExt(); i++)
        {
            ext_cs.Points(i, p1, p2);

            //ps1.emplace_back(p1.Value());
            //ps2.emplace_back(p2.Value());
        }
    }
    return std::clock() - start;
}

int TestCurveSurfIntersectOCC(const Handle(Geom_BSplineCurve)& c, const Handle(Geom_Surface)& s,
                              std::vector<gp_Pnt>& ps1, std::vector<gp_Pnt>& ps2)
{
    GeomAdaptor_Curve ac(c);
    GeomAdaptor_Surface as(s);
    return TestCurveSurfIntersectAdaptor(ac, as, ps1, ps2);
}

int TestCurveSurfIntersectMethod1(const Handle(Geom_Curve)& c, const Handle(Geom_Surface)& s,
                                  std::vector<gp_Pnt>& ps1, std::vector<gp_Pnt>& ps2)
{
    GeomAdaptor_Curve ac(c);
    GeomAdaptor_Surface as(s);
    return TestCurveSurfIntersectAdaptor(ac, as, ps1, ps2);
}

int TestCurveSurfIntersectMethod2(const Handle(BRep_CurveOnSurface)& c, const Handle(Geom_Surface)& s,
                                  std::vector<gp_Pnt>& ps1, std::vector<gp_Pnt>& ps2)
{
    Handle(Geom2dAdaptor_Curve) apc = new Geom2dAdaptor_Curve(c->PCurve());
    Handle(GeomAdaptor_Surface) acs = new GeomAdaptor_Surface(c->Surface());
    Adaptor3d_CurveOnSurface ac(apc, acs);

    GeomAdaptor_Surface as(s);
    return TestCurveSurfIntersectAdaptor(ac, as, ps1, ps2);
}

void PrintTimeStat(const std::string& name, const std::vector<int>& times)
{
    if (times.empty())
    {
        std::cout << name << ": No data." << std::endl;
        return;
    }
    int total = 0;
    int min_time = times[0];
    int max_time = times[0];
    for (const int t : times)
    {
        total += t;
        if (t < min_time) min_time = t;
        if (t > max_time) max_time = t;
    }
    double avg_time = static_cast<double>(total) / times.size();
    std::cout << name << " - Count: " << times.size()
        << ", Min: " << min_time << " ms"
        << ", Max: " << max_time << " ms"
        << ", Avg: " << avg_time << " ms" << std::endl;
}

void TestHelix::TestPerf(std::vector<TopoDS_Shape>& shapes, std::vector<std::string>& names)
{
    const int N = 500;
    HelixData helix_data;
    helix_data.rStart = 10;
    helix_data.taperFactor = 0;
    helix_data.pitch = 5;
    helix_data.ccw = true;
    helix_data.tStart = 0;
    helix_data.tEnd = 2 * M_PI;
    helix_data.loc = gp_Ax2(gp_Pnt(1, 1, 1), gp::DX(), gp::DZ());
    //helix_data.loc = gp::XOY();

    const Handle(Adaptor3d_Curve) occ_adaptor_curve = MakeHelixAdaptorCurveOCC(helix_data);

    const Handle(Geom_BSplineCurve) helix_curve_occ = MakeHelixCurveOCC(helix_data);
    const Handle(Geom_Curve) helix_curve_1 = MakeHelixCurve1(helix_data);
    const Handle(BRep_CurveOnSurface) helix_curve_2 = MakeHelixCurve2(helix_data);

    const Handle(Geom_Curve) base_curve = MakeBaseCurve();
    const Handle(Geom_Surface) base_surf = MakeBaseSurf();

    // 输出模型
    MakePolygon(occ_adaptor_curve, "OCC 8.0 螺旋线采样点", shapes, names);
    MakeEdge(helix_curve_occ, "OCC 8.0 螺旋线", shapes, names);
    MakeEdge(helix_curve_1, "Method 1 螺旋线", shapes, names);
    MakeEdge(helix_curve_2, "Method 2 螺旋线", shapes, names);
    MakeEdge(base_curve, "基准线-用于求距离", shapes, names);
    MakeFace(base_surf, "基准面-用于求交", shapes, names);

    // 线-线距离
    std::srand(std::clock());
    std::vector<int> occ_times;
    std::vector<int> method1_times;
    std::vector<int> method2_times;
    std::vector<gp_Pnt> ps1, ps2;
    for (int i = 0; i < N; i++)
    {
        int time = 0;
        const int algo = std::rand() % 3;
        switch (algo)
        {
            case 0: // occ-method
            {
                time = TestCurveCurveDistOCC(helix_curve_occ, base_curve, ps1, ps2);
                occ_times.emplace_back(time);
                MakeLines(ps1, ps2, "Dist OCC", shapes, names);
            } break; // 1-method
            case 1:
            {
                time = TestCurveCurveDistMethod1(helix_curve_1, base_curve, ps1, ps2);
                method1_times.emplace_back(time);
                MakeLines(ps1, ps2, "Dist 1", shapes, names);
            } break;
            default: // 2-method
            {
                time = TestCurveCurveDistMethod2(helix_curve_2, base_curve, ps1, ps2);
                method2_times.emplace_back(time);
                MakeLines(ps1, ps2, "Dist 2", shapes, names);
            } break;
        }
    }

    PrintTimeStat("Dist OCC-method", occ_times);
    PrintTimeStat("Dist 1-method", method1_times);
    PrintTimeStat("Dist 2-method", method2_times);

    // 线-面求交
    occ_times.clear();
    method1_times.clear();
    method2_times.clear();
    for (int i = 0; i < N; i++)
    {
        int time = 0;
        const int algo = std::rand() % 3;
        switch (algo)
        {
            case 0: // occ-method
            {
                time = TestCurveSurfIntersectOCC(helix_curve_occ, base_surf, ps1, ps2);
                occ_times.emplace_back(time);
                MakeVertices(ps1, "Intersect OCC", shapes, names);
            } break;
            case 1: // 1-method
            {
                time = TestCurveSurfIntersectMethod1(helix_curve_1, base_surf, ps1, ps2);
                method1_times.emplace_back(time);
                MakeVertices(ps1, "Intersect 1", shapes, names);
            } break;
            default: // 2-method
            {
                time = TestCurveSurfIntersectMethod2(helix_curve_2, base_surf, ps1, ps2);
                method2_times.emplace_back(time);
                MakeVertices(ps1, "Intersect 2", shapes, names);
            } break;
        }
    }

    PrintTimeStat("Intersect OCC-method", occ_times);
    PrintTimeStat("Intersect 1-method", method1_times);
    PrintTimeStat("Intersect 2-method", method2_times);
}

void TestHelix::Examples(std::vector<TopoDS_Shape>& shapes, std::vector<std::string>& names)
{
    HelixData helix_data;
    helix_data.loc = gp::XOY();
    helix_data.ccw = true;
    {
        helix_data.rStart = 10;
        helix_data.taperFactor = 0;
        helix_data.pitch = 5;
        helix_data.tStart = 0;
        helix_data.tEnd = 6 * M_PI;

        auto c = MakeHelixAdaptorCurveOCC(helix_data);
        MakePolygon(c, "rStart=10, pitch=5, 6PI", shapes, names);
    }
    {
        helix_data.rStart = 10;
        helix_data.taperFactor = 0;
        helix_data.pitch = 12;
        helix_data.tStart = 0;
        helix_data.tEnd = 6 * M_PI;

        auto c = MakeHelixAdaptorCurveOCC(helix_data);
        MakePolygon(c, "rStart=10, pitch=12, 6PI", shapes, names);
    }
    {
        helix_data.rStart = 10;
        helix_data.taperFactor = 1;
        helix_data.pitch = 12;
        helix_data.tStart = 0;
        helix_data.tEnd = 6 * M_PI;

        auto c = MakeHelixAdaptorCurveOCC(helix_data);
        MakePolygon(c, "rStart=10, taperF=1, pitch=12, 6PI", shapes, names);
    }
    {
        helix_data.rStart = 12;
        helix_data.taperFactor = 3;
        helix_data.pitch = 12;
        helix_data.tStart = 0;
        helix_data.tEnd = 6 * M_PI;

        auto c = MakeHelixAdaptorCurveOCC(helix_data);
        MakePolygon(c, "rStart=20, taperF=0.5, pitch=12, 6PI", shapes, names);
    }
}
